/*
 * CanBotLL.h
 *
 *  Created on: 6-mrt.-2014
 *      Author: thomas
 */

#ifndef CANBOTLL_H_
#define CANBOTLL_H_

#include "Serial.h"
#include <stdlib.h>
#include <unistd.h>
#include <cmath>

#define LOW_SPEED 50
#define MAX_SPEED 200

const int CLAW_OPEN = 1;
const int CLAW_CLOSE = 0;
//Drempelwaarden voor snelheid:
#define ANGLE_HIGH_SPEED 10
#define DIST_HIGH_SPEED 20 //cm

//Duurtijd voor motoren:
//#define US_10DG_HS 1000
#define US_1DG_HS 9625
//NOG BEREKENEN
//#define US_1DG_LS 1000
#define US_1UNIT_HS 10000

/*!  \class CanBotLL
 	 \brief CanBotLL is the Low Level implementation on CanBot
 	 	 	This class is responsible for the serial communication with the robot
*/
class CanBotLL {
private:
	Serial *ser_robot;
	void driveMotor(int speedL, int speedR);
	string serial_read;

public:
	/**
	 * 	CanBotLL constructor
	 * 	This constructor initializes a serial connection.
	 *  Next it will wait for the user to press START
	 */
	CanBotLL();
	/**
	 * 	CanBotLL destructor
	 * 	It will do no more then closing the serial port
	 */
	virtual ~CanBotLL();

	/**
	* 	This function drives the robots claw
	* 	@param open_close int which describes the movement please use CLAW_OPEN and CLAW_CLOSE (defined const)
	*/
	int claw(int open_close);

	/**
	* 	function meant to turn over a predefined angle
	*
	* 	@param angle int which describes the angle to turn
	* 	(negative to turn clockwise, positive to turn counter clockwise)
	* 	This function uses an assumption for the time (us) needed to turn 1deg (US_1DG_HS)
	*/
	int turn(int angle);

	/**
	* 	function meant to drive over a predefined distance
	*
	* 	@param distance int which describes the distance to drive
	* 	(negative to drive backwards, positive to drive forward)
	* 	This function uses an assumption for the time (us) needed to drive 1px (US_1UNIT_HS)
	*/
	int drive(int distance);

	/**
	* 	function meant to catch a can
 	*
	* 	Before this function is called it is necessary that a can is in front of the robot
	* 	The robot will drive till a can is between the claw, closes the claw
	*/
	int catchCan();

	/**
	* 	function meant to read out the distance sensor
	*	it returns the read value
	*/
	int distance();

	//tweede instantie
	/**
	* 	function starts turning the robot
	*	@param dir int which describes the direction to turn
	*	(0 to turn clockwise, 1 to turn counter clockwise)
	*	it is important to call movementStop() to stop the robots turning
	*	@see driveStart(int dir)
	*	@see movementStop()
	*/
	int turnStart(int dir); //0 rechts 1 links

	/**
	* 	function starts the robot driving
	*	@param dir int which describes the direction to drive
	*	(0 to drive backwards, 1 to drive forward)
	*	it is important to call movementStop() to stop the robots driving
	*	@see turnStart(int dir)
	*	@see movementStop()
	*/
	int driveStart(int dir);//0 achter 1 voor

	/**
	* 	function stops the robot from moving
	*	this function is called after turnStart or driveStart
	*	@see turnStart(int dir)
	*	@see driveStart(int dir)
	*/
	int movementStop();
};

#endif /* CANBOTLL_H_ */
